Feature/gradient occupancy#66
Conversation
|
@nathanhhughes The old tsdf occupancy publisher broke when there were ramps and hills, causing some issues with the mid-level planner. I'm not sure if these changes are ready to be merged and how we should try that in the DCSIT system. So I thought it was best to pin you to get an idea. |
|
@MultyXu yeah, this will need some work on the Hydra side to actually merge in (it doesn't make sense to duplicate the code and work of the gradient-based traversability estimator), but easy to keep rebasing on top of develop if we have to make other changes to Hydra-Ros |
MultyXu
force-pushed
the
feature/gradient_occupancy
branch
from
e237833 to
5092844
Compare
nathanhhughes
left a comment
nathanhhughes
left a comment
There was a problem hiding this comment.
I think there's still too much duplication with the code and computation in Hydra and we should fix that before merging this
| float confidence = 0.0f; // num_neighbors / 8.0 | ||
| }; | ||
|
|
||
| class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { |
There was a problem hiding this comment.
I think we should just add this as a sink to the traversability estimator instead of the reconstruction module (which would help drop the requirement for most of the code duplication), but we can also talk about what kind of architecture makes sense (we can also think about publishing this as a cost map instead of a occupancy grid and refining the mid-level planner to be able to take that as well)
|
Will need this PR |
nathanhhughes
left a comment
nathanhhughes
left a comment
There was a problem hiding this comment.
Looks great to me! Not a big deal that this is specialized to the gradient-based estimator at the moment, we can tweak what the sink looks like later if it turns out we want to have a more general "cost map" publisher
| if (config.add_robot_footprint) { | ||
| const Eigen::Vector3f pos_3d(pos.x(), pos.y(), 0.0f); | ||
| if (bbox.contains((sensor_T_world * pos_3d).eval())) { | ||
| msg.data[index] = 0; | ||
| continue; | ||
| } | ||
| } | ||
|
|
||
| msg.data[index] = | ||
| gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); | ||
| } |
There was a problem hiding this comment.
Technically these should be swapped (the idea being that the footprint is a pessimistic volume that overrides cells to be free if the robot is currently occupying them), but this is not a big deal either way
2 participants
Add a gradient-based map publisher for the mid-level planner and other functionalities.